#pragma once

#include "g2o/core/base_multi_edge.h"
#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_vec.hpp"
#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_q.hpp"
#include "lidar_localization/models/graph_optimizer/preintegration/preintegration.hpp"

namespace g2o
{
    class EdgeImu : public BaseMultiEdge<15, std::shared_ptr<lidar_localization::PreIntegration>>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgeImu();
        virtual bool read(std::istream &is) { return false; }
        virtual bool write(std::ostream &os) const { return false; }
        void computeError();
        virtual void linearizeOplus();
        virtual void setMeasurement(const std::shared_ptr<lidar_localization::PreIntegration> &m);
        std::vector<g2o::MatrixX::MapType, Eigen::aligned_allocator<g2o::MatrixX::MapType>> GetJacobian() const
        {
            return _jacobianOplus;
        }
    };
} // namespace g2o